// Copyright (c) 2022.
// Written by Li Yanxiao. All rights reserved
#include <cmath>
#include <iostream>
#include "distance.h"

using std::cout;
using std::endl;

inline bool OverStart(const Eigen::Vector2i& point, const Line &line) {
    return (point - line.getStartPoint()).dot(line.vector()) < 0;
}

inline bool OverEnd(const Eigen::Vector2i &point, const Line &line) {
    return (point - line.getEndPoint()).dot(line.vector()) > 0;
}

inline double DistancePointToLine(const Eigen::Vector2i &point, const Line &line) {
    return abs(point.x() + line.getB() * point.y() + line.getC()) /
            sqrt(1 + line.getB() * line.getB());
}

inline double DistancePointToPoint(const Eigen::Vector2i &point1, const Eigen::Vector2i &point2) {
    return sqrt(pow(point1.x() - point2.x(), 2) +
    pow(point1.y() - point2.y(), 2));
}

double DistancePointToLineSegment(const Eigen::Vector2i &point, const Line &line) {
    if (OverStart(point, line)){
        return DistancePointToPoint(point, line.getStartPoint());
    }
    else if (OverEnd(point, line)){
        return DistancePointToPoint(point, line.getEndPoint());
    }
    else{
        return DistancePointToLine(point, line);
    }
}

double MinDistance(const Eigen::Vector2i &point, const Line *line) {
    assert(line != nullptr); // 空指针检测
    double distances[3];
    distances[0] = DistancePointToLineSegment(point, line[0]);
    distances[1] = DistancePointToLineSegment(point, line[1]);
    distances[2] = DistancePointToLineSegment(point, line[2]);

    double min_value = distances[0];
    int min_place = 0;
    int i;
    for (i = 0; i < 3; i++) {
        if (distances[i] <= min_value) {
            min_value = distances[i];
            min_place = i;
        }
    }

    if (min_place == 2 && abs(point.y() - line[2].y(point.x())) <= 5 && OverEnd(point, line[2])){
        min_value = DistancePointToLine(point, line[2]);
    }
    if (min_place == 0 && abs(point.y() - line[0].y(point.x())) <= 5 && OverStart(point, line[0])){
        min_value = DistancePointToLine(point, line[0]);
    }
    //染色：通过叉乘定义距离的正负值
    Eigen::Vector2i point_to_line = point - line[min_place].getStartPoint();
    Eigen::Vector3i temp_vector_1(line[min_place].vector().x(), line[min_place].vector().y(), 0);
    Eigen::Vector3i temp_vector_2(point_to_line.x(), point_to_line.y(), 0);
    Eigen::Vector3i temp_vector_3;

    temp_vector_3 = temp_vector_1.cross(temp_vector_2);
    if (temp_vector_3.z() > 0) {
        min_value = -min_value;
    }

    if (min_place == 2 && (OverEnd(point, line[1]) and OverStart(point, line[2]))) {
        Eigen::Vector3i temp_vector_4(line[1].vector().x(), line[1].vector().y(), 0);
        Eigen::Vector2i point_to_line2 = point - line[1].getStartPoint();
        Eigen::Vector3i temp_vector_5(point_to_line.x(), point_to_line.y(), 0);
        Eigen::Vector3i temp_vector_6 = temp_vector_4.cross(temp_vector_5);
        if (temp_vector_6.z() > 0) {
            min_value = -abs(min_value);
        } else {
            min_value = abs(min_value);
        }
        if (!Cross2(line[1].vector(), line[2].vector())) {
            if (temp_vector_3.z() < 0 && temp_vector_6.z() > 0) {
                min_value = -abs(min_value);
            }
            if (temp_vector_3.z() > 0 && temp_vector_6.z() < 0) {
                min_value = -abs(min_value);
            }
        } else {
            if (temp_vector_3.z() < 0 && temp_vector_6.z() > 0) {
                min_value = abs(min_value);
            }
            if (temp_vector_3.z() > 0 && temp_vector_6.z() < 0) {
                min_value = abs(min_value);
            }
        }

    }

    return min_value;
}

Eigen::Vector2i* MinPoint(Eigen::Vector2i *dst, const Eigen::Vector2i &point, const Line *line) {
    assert(line != nullptr and dst != nullptr); // 空指针检测
    double distances[3];
    distances[0] = DistancePointToLineSegment(point,line[0]);
    distances[1] = DistancePointToLineSegment(point,line[1]);
    distances[2] = DistancePointToLineSegment(point,line[2]);

    double min_value = distances[0];
    int min_place = 0;
    int i;
    for (i=0;i<3;i++){
        if (distances[i] < min_value){
            min_value = distances[i];
            min_place = i;
        }
    }

    if (OverStart(point, line[min_place])){
        *dst = line[min_place].getStartPoint();
    }
    else if (OverEnd(point, line[min_place])){
        *dst = line[min_place].getEndPoint();
    }
    else{
        double b = line[min_place].getB();
        double c = line[min_place].getC();
        double x = floor((pow(b,2) * point.x() - b * point.y() - c)
                / (1 + pow(b,2)));
        double y = floor((point.y() - b * point.x() - b * c) / (1 + pow(b,2)));

        *dst = Eigen::Vector2i(x,y);
    }

    return dst;
}

Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>* MinDistanceList(Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>*dst,
                                                                       const Line *line, int rows, int cols) {
    for (int i=0;i<rows;i = i + 2){
        for (int j=0;j<cols;j = j + 2){
            double min = MinDistance(Eigen::Vector2i(j, i), line);
            (*dst)(i, j) = min;
            (*dst)(i, j+1) = min;
            (*dst)(i+1, j) = min;
            (*dst)(i+1, j+1) = min;
        }
    }
    return dst;
}

bool Cross2(Eigen::Vector2i vec1, Eigen::Vector2i vec2) {
    Eigen::Vector3i temp_1(vec1.x(),vec1.y(), 0);
    Eigen::Vector3i temp_2(vec2.x(),vec2.y(), 0);
    return (temp_1.cross(temp_2).z() >= 0);
}
